from launch import LaunchDescription
from launch_ros.actions import Node
import os
import yaml

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.actions import Node

launch_params = yaml.safe_load(open(os.path.join(
    get_package_share_directory('rm_vision_bringup'), 'config', 'launch_params.yaml')))
robot_type_param = yaml.safe_load(open(os.path.join(
    get_package_share_directory('rm_vision_bringup'), 'config', 'robot_type.yaml')))

this_robot_tfs = launch_params[robot_type_param["robot_type"]]

rpyparams = this_robot_tfs['imu2gimbal_link']['rpy']
# 去除两边的引号，并通过空格分割，转化为小数
rpyparams = rpyparams.strip('\"').split(' ')
rpyparams = [float(i) for i in rpyparams]
# print(rpyparams)

def generate_launch_description():

    odom_tf=Node(
        package="sen_ug_ctr",
        executable="sen_ug_ctr_node",
        name='sen_ug_ctr_node',
        parameters=[{
                    #  roll pitch yaw
                    'roll': rpyparams[0],
                    'pitch': rpyparams[1],
                    'yaw': rpyparams[2],
        }],
    )

    launch_description =LaunchDescription([odom_tf, driver_node])
    return launch_description
